/* 
 * File: main.c
 * Author: Testuya Idota and Brian Fehrman
 * Description: Sends commands to the iRobot
 * 				from the Seeduino via USART
 * 				communication. The robot will
 * 				be commanded to drive in a straight line,
 * 				when it hits an obstacle, goes back and turn arround.
 * Created on (date): 2-5-2013
 * Updated on (date): 2-12-2013
 */
#include <avr/io.h>
#include <util/delay.h>
#include <inttypes.h>

#include "iRobotControl.h"
#include "usart.h"

int main (void)
{
	// call usart initialize
	usart_config ucon;
	ucon.number = 1;
	ucon.baudrate = 57600;
	configure_usart(&ucon);
    
	_delay_ms(1000); //wait for a second

	usart1tx((uint8_t)128); //Start
	
	_delay_ms(1000); //wait for a second
	
	usart1tx((uint8_t)132); //to Full Mode
	
	_delay_ms(1000);
	

	while(1){
		uint8_t status; //buffer of data from the robot

		//go straight
		DriveRL(100,100);
		
		do{
			_delay_ms(50);
			//Check the status
			status = BumpsAndWheelDrops();
		}while((status & 0x03) == 0); //during no bump
		_delay_ms(30);		
	
		//back
		DriveDist(-100,-100);
		_delay_ms(1100);
		
		//turn
		if((status & 0x03) == 0x03) //front bump
			TurnAngle(-90);
		else if((status & 0x03) == 0x01) //right bump
			TurnAngle(90);
		else //left bump
			TurnAngle(-90);
		_delay_ms(3000);
		
	}

	return -1;
}
